
#include "full_cover_path_planner/polygon2d.h"
#include "common/math_utils.h"
#include "common/constants.h"

using namespace global_path_planner::common;

namespace global_path_planner
{
    namespace ccpp
    {
        // 点在多边形的边上或内部
        bool Polygon2D::pointInPolygon(const Polygon_2 &poly, const Point_2 &p)
        {
            CGAL::Bounded_side result =
                CGAL::bounded_side_2(poly.vertices_begin(),
                                     poly.vertices_end(), p, K());
            if (result == CGAL::ON_UNBOUNDED_SIDE)
                return false;
            return true;
        }

        bool Polygon2D::pointsInPolygon(const Polygon_2 &poly, const std::vector<Point_2> &pts)
        {
            for (auto it = pts.begin(); it != pts.end(); ++it)
            {
                if (!pointInPolygon(poly, *it))
                    return false;
            }
            return true;
        }

        // 判断多边形是否有多个交叉点
        bool Polygon2D::isStrictlySimple(const Polygon_2 &poly)
        {
            return poly.is_simple();
        }

        // 创建polygon2
        bool Polygon2D::createPolygon2(Polygon_2 *poly)
        {
            Polygon_2::Traits::Equal_2 eq_2;
            bool erase_one = true;
            while (erase_one)
            {
                Polygon_2::Vertex_circulator vit = poly->vertices_circulator();
                erase_one = false;
                do
                {
                    if (eq_2(*vit, *std::next(vit)))
                    {
                        poly->erase(vit++);
                        erase_one = true;
                    }
                    else
                        vit++;
                } while (vit != poly->vertices_circulator());
            }
            return computeArea(*poly) > 20.0;
        }

        // 点映射到多边形最长距离.
        std::pair<FT, Point_2> Polygon2D::projectOnPolygon2(const Polygon_2 &poly, const Point_2 &p)
        {
            // Find the closest edge.
            std::vector<std::pair<FT, EdgeConstIterator>> edge_distances(poly.size());
            std::vector<std::pair<FT, EdgeConstIterator>>::iterator dit =
                edge_distances.begin();
            for (EdgeConstIterator eit = poly.edges_begin();
                 eit != poly.edges_end();
                 eit++, dit++)
            {
                dit->first = CGAL::squared_distance(*eit, p);
                dit->second = eit;
            }

            std::vector<std::pair<FT, EdgeConstIterator>>::iterator closest_pair =
                std::min_element(edge_distances.begin(), edge_distances.end(),
                                 [](const std::pair<FT, EdgeConstIterator> &lhs,
                                    const std::pair<FT, EdgeConstIterator> &rhs)
                                 { return lhs.first > rhs.first; });

            EdgeConstIterator closest_edge = closest_pair->second;
            // FT squared_distance = closest_pair->first;
            // Project p on supporting line of closest edge.
            Point_2 projection = closest_edge->supporting_line().projection(p);
            // Check if p is on edge. If not snap it to source or target.
            // if (!closest_edge->has_on(projection))
            // {
            //     FT d_source = CGAL::squared_distance(p, closest_edge->source());
            //     FT d_target = CGAL::squared_distance(p, closest_edge->target());
            //     projection =
            //         d_source < d_target ? closest_edge->source() : closest_edge->target();
            // }

            return std::make_pair(closest_pair->first, projection);
        }

        //  计算点到多边形的最小距离
        FT Polygon2D::minDistOfPonit2ToPolygon2(const Polygon_2 &poly, const Point_2 &p)
        {
            // Find the closest edge.
            std::vector<std::pair<FT, EdgeConstIterator>> edge_distances(poly.size());
            std::vector<std::pair<FT, EdgeConstIterator>>::iterator dit =
                edge_distances.begin();
            for (EdgeConstIterator eit = poly.edges_begin();
                 eit != poly.edges_end();
                 eit++, dit++)
            {
                dit->first = CGAL::squared_distance(*eit, p);
                dit->second = eit;
            }

            std::vector<std::pair<FT, EdgeConstIterator>>::iterator closest_pair =
                std::min_element(edge_distances.begin(), edge_distances.end(),
                                 [](const std::pair<FT, EdgeConstIterator> &lhs,
                                    const std::pair<FT, EdgeConstIterator> &rhs)
                                 { return lhs.first < rhs.first; });

            return closest_pair->first;
        }

        // 计算多边形的面积
        FT Polygon2D::computeArea(const Polygon_2 &poly)
        {
            FT area =
                CGAL::abs(CGAL::polygon_area_2(poly.vertices_begin(),
                                               poly.vertices_end(), K()));
            return area;
        }

        // 删除共线的顶点
        void Polygon2D::removeCollinearVertices(Polygon_2 *polygon)
        {
            std::vector<Polygon_2::Vertex_circulator> v_to_erase;

            Polygon_2::Vertex_circulator vc = polygon->vertices_circulator();
            // Find collinear vertices.
            do
            {
                if (CGAL::collinear(*std::prev(vc), *vc, *std::next(vc)))
                {
                    v_to_erase.push_back(vc);
                }
            } while (++vc != polygon->vertices_circulator());

            // Remove intermediate vertices.
            std::vector<Polygon_2::Vertex_circulator>::reverse_iterator rit = v_to_erase.rbegin();
            for (; rit != v_to_erase.rend(); ++rit)
            {
                polygon->erase(*rit);
            }
        }

        // 旋转多边形
        Polygon_2 Polygon2D::rotatePolygon(const Polygon_2 &poly,
                                           const Direction_2 &dir)
        {
            CGAL::Aff_transformation_2<K> rotation(CGAL::ROTATION, dir, 1, 1e9);
            Polygon_2 rotated_polygon;
            rotation = rotation.inverse();
            rotated_polygon = CGAL::transform(rotation, poly);
            return rotated_polygon;
        }

        // 判断多边形的顺逆性
        bool Polygon2D::IsClockwise(Polygon_2 &poly)
        {
            // 1. 找到横坐标最小的点，该点一定是多边形的最左侧的点，且为凸点
            VertexIterator min_vertex = poly.vertices_begin();
            double min_x = CGAL::to_double(poly.vertices_begin()->x());
            for (auto it = poly.vertices_begin() + 1; it != poly.vertices_end(); it++)
            {
                if (CGAL::to_double(it->x()) < min_x)
                {
                    min_x = CGAL::to_double(it->x());
                    min_vertex = it;
                }
            }

            // 2. 判断该点与前一个点构成的向量 和 与后一个点构成的向量的叉积的正负
            // 2.1 获取该点前一个点和后一个点的下标
            VertexIterator prev_min_vertex, next_min_vertex;
            if (poly.vertices_begin() == min_vertex)
            { // 如果该点是第一个点
                prev_min_vertex = poly.vertices_end() - 1;
                next_min_vertex = min_vertex + 1;
            }
            else if (poly.vertices_end() - 1 == min_vertex)
            { // 如果该点是最后一个点
                prev_min_vertex = min_vertex - 1;
                next_min_vertex = poly.vertices_begin();
            }
            else
            {
                prev_min_vertex = min_vertex - 1;
                next_min_vertex = min_vertex + 1;
            }

            // 2.2 计算叉积
            double cross = (CGAL::to_double(prev_min_vertex->x()) - CGAL::to_double(min_vertex->x())) *
                               (CGAL::to_double(next_min_vertex->y()) - CGAL::to_double(min_vertex->y())) -
                           (CGAL::to_double(next_min_vertex->x()) - CGAL::to_double(min_vertex->x())) *
                               (CGAL::to_double(prev_min_vertex->y()) - CGAL::to_double(min_vertex->y()));

            if (cross > 0)
                return true;
            else
                return false;
        }

        // 计算多边形之间是否存在交叉
        std::pair<std::vector<std::pair<EdgeConstIterator, Point_2>>,
                  std::vector<std::pair<EdgeConstIterator, Point_2>>>
        Polygon2D::PolygonsIntersection(Polygon_2 &out_polys, Polygon_2 &hole_polys)
        {
            std::vector<std::pair<EdgeConstIterator, Point_2>> hole_intersect_edges;
            std::vector<std::pair<EdgeConstIterator, Point_2>> out_intersect_edges;
            std::pair<std::vector<std::pair<EdgeConstIterator, Point_2>>,
                      std::vector<std::pair<EdgeConstIterator, Point_2>>>
                res_insects;
            for (auto out_it = out_polys.edges_begin(); out_it != out_polys.edges_end(); out_it++)
            {
                for (auto hole_it = hole_polys.edges_begin(); hole_it != hole_polys.edges_end(); hole_it++)
                {
                    typedef CGAL::cpp11::result_of<Intersect_2(Segment_2, Segment_2)>::type
                        Intersection;
                    Intersection result = CGAL::intersection(*out_it, *hole_it);
                    Point_2 pt;
                    if (result)
                    {
                        if (const Segment_2 *s = boost::get<Segment_2>(&*result))
                        {
                            continue;
                        }
                        else
                        {
                            bool is_exit = false;
                            pt = *boost::get<Point_2>(&*result);
                            // 面积小的多边形缓存交叉点
                            if (hole_intersect_edges.size() == 0)
                                hole_intersect_edges.emplace_back(hole_it, pt);
                            else
                            {
                                for (auto i = 0; i < hole_intersect_edges.size(); i++)
                                {
                                    if (pt == hole_intersect_edges[i].second &&
                                        ((hole_intersect_edges[i].first->source() == hole_it->source() &&
                                          hole_intersect_edges[i].first->target() == hole_it->target()) ||
                                         (hole_intersect_edges[i].first->target() == hole_it->source() &&
                                          hole_intersect_edges[i].first->source() == hole_it->target())))
                                    {
                                        is_exit = true;
                                        break;
                                    }
                                }
                                if (!is_exit)
                                    hole_intersect_edges.emplace_back(std::make_pair(hole_it, pt));
                            }
                            is_exit = false;
                            // 面积大的多边形缓存交叉点
                            if (out_intersect_edges.size() == 0)
                                out_intersect_edges.emplace_back(out_it, pt);
                            else
                            {
                                for (auto i = 0; i < out_intersect_edges.size(); i++)
                                {
                                    if (pt == out_intersect_edges[i].second &&
                                        ((out_intersect_edges[i].first->source() == out_it->source() &&
                                          out_intersect_edges[i].first->target() == out_it->target()) ||
                                         (out_intersect_edges[i].first->target() == out_it->source() &&
                                          out_intersect_edges[i].first->source() == out_it->target())))
                                    {
                                        is_exit = true;
                                        break;
                                    }
                                }
                                if (!is_exit)
                                    out_intersect_edges.emplace_back(std::make_pair(out_it, pt));
                            }
                        }
                    }
                }
            }
            res_insects = std::make_pair(out_intersect_edges, hole_intersect_edges);
            return res_insects;
        }

        // 生成单一结构的多多边形
        std::vector<Polygon_2> Polygon2D::GenSimplePolys(std::vector<Polygon_2> &not_simple_polys, bool is_out_poly)
        {
            std::vector<Polygon_2> res_simple_polys;
            while (not_simple_polys.size() != 0)
            {
                auto tmp_poly = not_simple_polys.front();
                int last_sz = tmp_poly.size();
                do
                {
                    if (last_sz < 3)
                        break;
                    removeCollinearVertices(&tmp_poly);
                    if (last_sz == tmp_poly.size())
                        break;
                    else
                        last_sz = tmp_poly.size();
                } while (1);
                if (tmp_poly.size() < 3)
                {
                    not_simple_polys.erase(not_simple_polys.begin());
                    continue;
                }
                if (!isStrictlySimple(tmp_poly))
                {
                    auto temp_polys = splitPolygonByInsection(tmp_poly, is_out_poly);
                    not_simple_polys.insert(not_simple_polys.end(), temp_polys.begin(), temp_polys.end());
                }
                else
                {
                    res_simple_polys.emplace_back(tmp_poly);
                }
                not_simple_polys.erase(not_simple_polys.begin());
            }
            return res_simple_polys;
        }

        // polygon是否存在交叉点
        std::vector<std::tuple<VertexConstIterator, VertexConstIterator, Point_2>>
        Polygon2D::PolygonIntersection(Polygon_2 &poly)
        {
            // 从多边形中找出非相邻边的交叉点，并进行排序
            std::vector<std::tuple<VertexConstIterator, VertexConstIterator, Point_2>> remove_vert_iters;
            for (auto cur_vert_it = poly.vertices_begin(); cur_vert_it != poly.vertices_end();)
            {
                if (cur_vert_it + 1 == poly.vertices_end())
                    break;
                if (cur_vert_it + 2 == poly.vertices_end())
                    break;
                bool has_insection = false;

                // first line
                Segment_2 cur_sge = Segment_2(*cur_vert_it, *(cur_vert_it + 1));
                for (auto next_vert_it = cur_vert_it + 2; next_vert_it != poly.vertices_end(); next_vert_it++)
                {
                    // second line
                    Segment_2 next_sge;
                    if (next_vert_it + 1 == poly.vertices_end() &&
                        cur_vert_it == poly.vertices_begin())
                        break;
                    else if (next_vert_it + 1 == poly.vertices_end() &&
                             cur_vert_it != poly.vertices_begin())
                    {
                        next_sge = Segment_2(*next_vert_it, *(poly.vertices_begin()));
                    }
                    else
                    {
                        next_sge = Segment_2(*next_vert_it, *(next_vert_it + 1));
                    }

                    typedef CGAL::cpp11::result_of<Intersect_2(Segment_2, Segment_2)>::type
                        Intersection;
                    Intersection result = CGAL::intersection(cur_sge, next_sge);
                    if (result)
                    {
                        Point_2 insect_pt;
                        if (const Segment_2 *s = boost::get<Segment_2>(&*result))
                        {
                            insect_pt = Point_2((CGAL::to_double(s->source().x()) + CGAL::to_double(s->target().x())) / 2.0,
                                                (CGAL::to_double(s->source().y()) + CGAL::to_double(s->target().y())) / 2.0);
                        }
                        else
                        {
                            insect_pt = *boost::get<Point_2>(&*result);
                        }
                        remove_vert_iters.push_back(std::make_tuple(cur_vert_it, next_vert_it, insect_pt));
                        cur_vert_it = next_vert_it + 1;
                        has_insection = true;
                        break;
                    }
                }
                if (!has_insection)
                    cur_vert_it++;
            }
            return remove_vert_iters;
        }

        // 凹边形转凸边形 --- 整合cell decomposer模块中
        std::vector<Polygon_2> Polygon2D::ConcaveToConvex(Polygon_2 &polygon, EdgeConstIterator &dir)
        {
            std::vector<Polygon_2> convex_polys;
            std::vector<Polygon_2> all_polys;
            all_polys.emplace_back(polygon);
            while (all_polys.size() != 0)
            {
                Polygon_2 poly = all_polys.back();
                all_polys.pop_back();

                removeCollinearVertices(&poly);

                auto no_convex_polys = createConvex(poly, dir, convex_polys);
                if (no_convex_polys.size() > 0)
                    all_polys.insert(all_polys.end(), no_convex_polys.begin(), no_convex_polys.end());
            }
            return convex_polys;
        }

        // 构造凸边形 --- 整合到cell decomposer模块
        std::vector<Polygon_2> Polygon2D::createConvex(Polygon_2 &polygon, EdgeConstIterator &dir, std::vector<Polygon_2> &convex_polys)
        {
            std::vector<Polygon_2> no_convex_polys;
            if (std::abs(CGAL::to_double(polygon.area())) < 25.0)
            {
                return no_convex_polys;
            }

            // 1.只有3个顶点构成的多边形作为凸边形
            if (polygon.size() == 3)
            {
                convex_polys.emplace_back(polygon);
                return no_convex_polys;
            }
            std::vector<Point_2> critical_points = getCriticalInflectionPoints(polygon);
            // 2.没有关键点构成的多边形作为凸边形
            if (critical_points.size() == 0)
            {
                convex_polys.emplace_back(polygon);
                return no_convex_polys;
            }
            // 3.选择离dir最近的边
            double min_dist = DBL_MAX;
            EdgeConstIterator edge_iter = polygon.edges_end();

            for (auto it = polygon.edges_begin(); it != polygon.edges_end(); it++)
            {
                auto theta_1 = std::abs(std::atan2(CGAL::to_double(dir->direction().dy()), CGAL::to_double(dir->direction().dx())));
                auto theta_2 = std::abs(std::atan2(CGAL::to_double(it->direction().dy()), CGAL::to_double(it->direction().dx())));
                if (std::abs(theta_1 - theta_2) < 1e-6)
                {
                    auto dist = CGAL::to_double(CGAL::squared_distance(*it, *dir));
                    if (dist < min_dist)
                    {
                        edge_iter = it;
                        min_dist = dist;
                    }
                }
            }

            if (edge_iter == polygon.edges_end())
            {
                convex_polys.emplace_back(polygon);
                return no_convex_polys;
            }

            // 4.找出离最长边的距离最短的点
            min_dist = DBL_MAX;
            Point_2 critical_vertice;
            for (auto i = 0; i < critical_points.size(); i++)
            {
                auto dist = CGAL::to_double(CGAL::squared_distance(critical_points[i], *edge_iter));
                if (dist < min_dist)
                {
                    critical_vertice = critical_points[i];
                    min_dist = dist;
                }
            }

            auto l0_x = CGAL::to_double(critical_vertice.x()) - CGAL::to_double(dir->direction().dx()) * 10;
            auto l0_y = CGAL::to_double(critical_vertice.y()) - CGAL::to_double(dir->direction().dy()) * 10;
            auto l1_x = CGAL::to_double(critical_vertice.x()) + CGAL::to_double(dir->direction().dx()) * 10;
            auto l1_y = CGAL::to_double(critical_vertice.y()) + CGAL::to_double(dir->direction().dy()) * 10;

            // 基于critical_vertex构造线段平行于最长边
            Segment_2 seg = Segment_2(Point_2(l0_x, l0_y),
                                      Point_2(l1_x, l1_y));

            // 线段与多边形交叉点
            std::vector<Point_2> intersection_points = getIntersectionPoints(polygon, seg);
            // 根据x/y排序
            sortVertices(intersection_points);
            // 根据critical分割多边形
            auto polys = splitPolygonByCritical(polygon, intersection_points, critical_vertice);
            if (polys.size() == 1)
            {
                convex_polys.emplace_back(polys[0]);
                return no_convex_polys;
            }
            no_convex_polys = polys;

            return no_convex_polys;
        }

        // 根据多边形交叉点分成多个多边形
        std::vector<Polygon_2> Polygon2D::splitPolygonByInsection(Polygon_2 &poly, bool is_out_poly)
        {
            // result Polygons
            std::vector<Polygon_2> res_polys;
            auto remove_vert_iters = PolygonIntersection(poly);
            if (remove_vert_iters.size() <= 0)
            {
                res_polys.emplace_back(poly);
                return res_polys;
            }
            // 主轮廓
            Polygon_2 main_poly;
            auto cur_it = poly.vertices_begin();
            for (auto remove_vert_it = remove_vert_iters.begin();
                 remove_vert_it != remove_vert_iters.end();
                 remove_vert_it++)
            {
                Polygon_2 remove_poly;
                while (cur_it <= std::get<0>(*remove_vert_it))
                {
                    main_poly.push_back(*cur_it);
                    cur_it++;
                }
                main_poly.push_back(std::get<2>(*remove_vert_it));
                remove_poly.push_back(std::get<2>(*remove_vert_it));
                for (; cur_it <= std::get<1>(*remove_vert_it); cur_it++)
                {
                    remove_poly.push_back(*cur_it);
                }
                if (remove_poly.size() > 2)
                    res_polys.emplace_back(remove_poly);
                cur_it = std::get<1>(*remove_vert_it) + 1;
                if (cur_it == poly.vertices_end())
                    break;
            }
            // 剩下的点集插入到主轮廓中
            while (cur_it != poly.vertices_end())
            {
                main_poly.push_back(*cur_it);
                cur_it++;
            }
            if (main_poly.size() > 2)
            {
                res_polys.emplace_back(main_poly);
            }
            // 当前为内轮廓时，分割后需要选择最大区域
            if (!is_out_poly)
            {
                Polygon_2 max_hole_polys = res_polys[0];
                FT max_area = computeArea(res_polys[0]);
                for (auto it = res_polys.begin() + 1; it != res_polys.end(); it++)
                {
                    if (max_area < computeArea(*it))
                    {
                        max_area = computeArea(*it);
                        max_hole_polys = *it;
                    }
                }
                res_polys.clear();
                res_polys.emplace_back(max_hole_polys);
            }
            return res_polys;
        }

        // 根据critical_vertice分割多边形
        std::vector<Polygon_2> Polygon2D::splitPolygonByCritical(const Polygon_2 &polygon, const std::vector<Point_2> &intersections, const Point_2 &critical_vertice)
        {
            std::vector<Polygon_2> split_polys;
            // 1.通过交叉点的第一个点和最后点构成一条直线
            Segment_2 split_line(intersections.front(), intersections.back());

            // 2.优先找出critival_vertice对应polygon位置
            VertexCirculator critical_iter;
            VertexCirculator iter = polygon.vertices_circulator();
            do
            {
                if (CGAL::to_double(iter->x()) == CGAL::to_double(critical_vertice.x()) &&
                    CGAL::to_double(iter->y()) == CGAL::to_double(critical_vertice.y()))
                {
                    critical_iter = iter;
                    break;
                }
            } while (++iter != polygon.vertices_circulator());

            // 3.critical_iter遍历顶点并根据顶点在左侧还是右侧，分割多边形
            Polygon_2 poly;
            poly.push_back(*iter);
            iter++;
            do
            {
                if ((!split_line.supporting_line().has_on_positive_side(*iter) &&
                     !split_line.supporting_line().has_on_positive_side(*(iter + 1))) ||
                    (split_line.supporting_line().has_on_positive_side(*iter) &&
                     split_line.supporting_line().has_on_positive_side(*(iter + 1))))
                    poly.push_back(*iter);
                else
                {
                    // 当前顶点与下个顶点位于split_line时保存poly，同时将poly最后的点作为下个poly的开始点
                    // 循环调用，直到回到critical_vertex
                    poly.push_back(*iter);
                    Segment_2 line(*iter, *(iter + 1));
                    for (auto i = 0; i < intersections.size(); i++)
                    {
                        if (line.has_on(intersections[i]))
                        {
                            poly.push_back(intersections[i]);
                            break;
                        }
                    }

                    if (CGAL::to_double(poly.vertices_begin()->x()) == CGAL::to_double((poly.vertices_end() - 1)->x()) &&
                        CGAL::to_double(poly.vertices_begin()->y()) == CGAL::to_double((poly.vertices_end() - 1)->y()))
                    {
                        poly.erase(poly.vertices_end() - 1);
                    }
                    auto end_point = *(poly.vertices_end() - 1);
                    if (poly.size() > 2)
                    {
                        split_polys.emplace_back(poly);
                    }
                    poly.clear();
                    if (split_polys.size() > 0)
                        poly.push_back(*(split_polys.back().vertices_end() - 1));
                    else
                        poly.push_back(end_point);
                }
            } while (++iter != critical_iter);
            // 最后将critical_vertex添加到poly中
            poly.push_back(*critical_iter);
            if (CGAL::to_double(poly.vertices_begin()->x()) == CGAL::to_double((poly.vertices_end() - 1)->x()) &&
                CGAL::to_double(poly.vertices_begin()->y()) == CGAL::to_double((poly.vertices_end() - 1)->y()))
            {
                poly.erase(poly.vertices_end() - 1);
            }
            if (poly.size() > 2)
            {
                split_polys.emplace_back(poly);
            }

            return split_polys;
        }

        // 移除非Simple polygon中点集---整合到cell decomposer模块中
        // std::vector<Polygon_2> Polygon2D::removeNotSimplePolygonPoints(Polygon_2 &polys, bool outer_hole, Point_2 start)
        // {
        //     std::vector<Polygon_2> res;
        //     auto all_polys = splitPolygonByInsection(polys);
        //     // 存在交叉点选择最大的区域
        //     FT max_area = 1e-6;
        //     FT min_dist = 1e+6;
        //     int index = -1;
        //     if (outer_hole)
        //     {
        //         for (auto i = 0; i < all_polys.size(); i++)
        //         {
        //             // if(all_polys[i].size() < 3) continue;
        //             FT dist = minDistOfPonit2ToPolygon2(all_polys[i], start);
        //             // FT dist;
        //             // projectOnPolygon2(all_polys[i], start, &dist);
        //             if (min_dist > dist)
        //             {
        //                 // res_polys = all_polys[i];
        //                 index = i;
        //                 min_dist = dist;
        //             }
        //         }
        //         res.emplace_back(all_polys[index]);
        //     }
        //     else
        //     {
        //         res = all_polys;
        //     }
        //     return res;
        // }

        // 根据x,y对顶点排序
        void Polygon2D::sortVertices(std::vector<Point_2> &vertices)
        {
            std::sort(vertices.begin(), vertices.end(), [](const Point_2 &lhs, const Point_2 &rhs)
                      {
                    // sort first using the x coordinate
                    if (!math::approximatelyEqual(CGAL::to_double(lhs.x()), CGAL::to_double(rhs.x()), constants::EPSILON))
                    {
                      return math::definitelyLessThan(CGAL::to_double(lhs.x()), CGAL::to_double(rhs.x()), constants::EPSILON);
                    }

                    // if x coordinate is the same, then sort using y coordinate
                    if (!math::approximatelyEqual(CGAL::to_double(lhs.y()), CGAL::to_double(rhs.y()), constants::EPSILON))
                    {
                      return math::definitelyLessThan(CGAL::to_double(lhs.y()), CGAL::to_double(rhs.y()), constants::EPSILON);
                    }

                    // else just return true, the two points are in the same place
                    return true; });
        }

        void Polygon2D::sortVertices(std::vector<VertexConstCirculator> &vertices)
        {
            // Sort x,y.
            Polygon_2::Traits::Less_xy_2 less_xy_2;
            std::sort(vertices.begin(), vertices.end(),
                      [&less_xy_2](const VertexConstCirculator &a,
                                   const VertexConstCirculator &b) -> bool
                      {
                          return less_xy_2(*a, *b);
                      });
        }

        // 获取多边形的顶点
        std::vector<Point_2> Polygon2D::getVertices(const Polygon_2 &poly)
        {
            std::vector<Point_2> vec(poly.size());
            std::vector<Point_2>::iterator vecit = vec.begin();
            for (VertexConstIterator vit = poly.vertices_begin();
                 vit != poly.vertices_end();
                 ++vit, ++vecit)
                *vecit = *vit;
            return vec;
        }

        // 计算多边形顶点到直线的距离，然后对顶点排序
        std::vector<Point_2> Polygon2D::sortVerticesToLine(const Polygon_2 &poly, const Line_2 &l)
        {
            // Copy points.
            std::vector<Point_2> pts(poly.size());
            std::vector<Point_2>::iterator pts_it = pts.begin();
            for (auto it = poly.vertices_begin(); it != poly.vertices_end(); ++it)
            {
                *(pts_it++) = *it;
            }

            // Sort.
            std::sort(pts.begin(), pts.end(),
                      [&l](const Point_2 &a, const Point_2 &b) -> bool
                      {
                          return CGAL::has_smaller_signed_distance_to_line(l, a, b);
                      });

            return pts;
        }

        // 获取segment --- 整合到cell planner模块中
        std::pair<bool, std::vector<Segment_2>> Polygon2D::getSegments(const Polygon_2 &poly, const Line_2 &line)
        {
            std::vector<Segment_2> segments;
            std::vector<Point_2> intersections = getIntersectionPoints(poly, line);
            // Sort.
            Line_2 perp_l = line.perpendicular(line.point(0));
            std::sort(intersections.begin(), intersections.end(),
                      [&perp_l](const Point_2 &a, const Point_2 &b) -> bool
                      {
                          return CGAL::has_smaller_signed_distance_to_line(perp_l, a, b);
                      });
            // 去重
            if (intersections.size() >= 2)
            {
                auto verbose = std::unique(intersections.begin(), intersections.end());
                intersections.erase(verbose, intersections.end());
            }

            // 统计Segments
            if(intersections.empty()) return std::make_pair(false, segments);
            else{
                for(auto i = 0; i < intersections.size() - 1; i++){
                   segments.emplace_back(intersections[i], intersections[i + 1]);
                }
            }
            return std::make_pair(true, segments);
        }

        // 线段与多边形相交
        std::vector<Point_2> Polygon2D::getIntersectionPoints(const Polygon_2 &poly, const Line_2 &line)
        {
            std::vector<Point_2> intersections;
            typedef CGAL::cpp11::result_of<Intersect_2(Segment_2, Line_2)>::type
                Intersection;

            for (EdgeConstIterator it = poly.edges_begin(); it != poly.edges_end(); ++it) {
              Intersection result = CGAL::intersection(*it, line);
              if (result) {
                if (const Segment_2* s = boost::get<Segment_2>(&*result)) {
                  intersections.push_back(s->source());
                  intersections.push_back(s->target());
                } else {
                  intersections.push_back(*boost::get<Point_2>(&*result));
                }
              }
            }
            return intersections;
        }

        std::vector<Point_2> Polygon2D::getIntersectionPoints(const Polygon_2 &polygon, const Segment_2 &segment)
        {
            std::vector<Point_2> intersections;
            std::set<Point_2> diff_intersect;

            for (auto edge_iterator = polygon.edges_begin(); edge_iterator != polygon.edges_end(); edge_iterator++)
            {
                Segment_2 edge = *edge_iterator;
                auto result = CGAL::intersection(edge, segment);

                if (result)
                {
                    auto edge_y_max = std::max(CGAL::to_double(edge.source().y()), CGAL::to_double(edge.target().y()));
                    auto edge_y_min = std::min(CGAL::to_double(edge.source().y()), CGAL::to_double(edge.target().y()));

                    if (auto seg = boost::get<Segment_2>(&*result))
                    {

                        diff_intersect.insert(seg->source());
                        diff_intersect.insert(seg->target());
                    }
                    else
                    {
                        auto pt = boost::get<Point_2>(&*result);
                        diff_intersect.insert(*pt);
                    }
                }
            }
            std::set<Point_2>::const_iterator itc = diff_intersect.cbegin();
            while (itc != diff_intersect.cend())
            {
                intersections.push_back(*itc);
                itc++;
            }
            return intersections;
        }

        // 查找点是否在多边形顶点中
        VertexIterator Polygon2D::findPointInPolygon2(const Point_2 &point, const Polygon_2 &poly)
        {
            VertexIterator find_iter = poly.vertices_end();
            for (auto iter = poly.vertices_begin(); iter != poly.vertices_end(); iter++)
            {
                if (math::approximatelyEqual(CGAL::to_double(iter->x()), CGAL::to_double(point.x()), constants::EPSILON) &&
                    math::approximatelyEqual(CGAL::to_double(iter->y()), CGAL::to_double(point.y()), constants::EPSILON))
                {
                    find_iter = iter;
                    break;
                }
            }
            return find_iter;
        }

        // 顺时针排序交叉点构成的边
        std::unordered_map<int, std::vector<std::pair<EdgeConstIterator, Point_2>>> Polygon2D::sortEdgeByIntersectPoints(std::vector<std::pair<EdgeConstIterator, Point_2>>& edge_insects){
            // out poly按照逆时针顺序排列
            std::unordered_map<int, std::vector<std::pair<EdgeConstIterator, Point_2>>> map_order_edge_insects;
            std::vector<std::pair<EdgeConstIterator, Point_2>> order_edge_insects;
            int idx = 0;
            while(edge_insects.size() > 0){
                int pre_sz = edge_insects.size();
                for(auto it = edge_insects.begin(); it != edge_insects.end(); ){
                    bool is_remove = false;
                    if(order_edge_insects.size() == 0){
                       order_edge_insects.emplace_back(*it);
                       it = edge_insects.erase(it);
                       is_remove = true;
                    }else{
                        auto iter = order_edge_insects.begin();
                        for(; iter != order_edge_insects.end(); iter++){
                            if((std::fabs(CGAL::to_double(iter->first->source().x()) - CGAL::to_double(it->first->source().x())) < 1e-6 &&
                               std::fabs(CGAL::to_double(iter->first->source().y()) - CGAL::to_double(it->first->source().y())) < 1e-6 &&
                               std::fabs(CGAL::to_double(iter->first->target().x()) - CGAL::to_double(it->first->target().x())) < 1e-6 && 
                               std::fabs(CGAL::to_double(iter->first->target().y()) - CGAL::to_double(it->first->target().y())) < 1e-6) ||
                               (std::fabs(CGAL::to_double(iter->first->source().x()) - CGAL::to_double(it->first->target().x())) < 1e-6 &&
                               std::fabs(CGAL::to_double(iter->first->source().y()) - CGAL::to_double(it->first->target().y())) < 1e-6))
                            {
                                order_edge_insects.insert(iter, *it);
                                it = edge_insects.erase(it);
                                is_remove = true;
                                break;
                            }else if(std::fabs(CGAL::to_double(iter->first->target().x()) - CGAL::to_double(it->first->source().x())) < 1e-6 &&
                                    std::fabs(CGAL::to_double(iter->first->target().y()) - CGAL::to_double(it->first->source().y())) < 1e-6)
                                    {
                                        order_edge_insects.insert(iter + 1, *it);
                                        it = edge_insects.erase(it);
                                        is_remove = true;
                                        break;
                                    }
                        }
                    }
                    if(!is_remove) it++;
                }
                
                if(pre_sz == edge_insects.size()) {
                    map_order_edge_insects[idx++] = order_edge_insects;
                    order_edge_insects.clear();
                }
            }

            if(order_edge_insects.size() > 0){
                map_order_edge_insects[idx++] = order_edge_insects;
            }

            return  map_order_edge_insects;
        }

        // 根据交叉点起点和终点顺时针重排顶点
        std::vector<Point_2> Polygon2D::sortVertexByIntersectPoints(Polygon_2& polys, Point_2& seg_start, Point_2& seg_end,
                                                                    Point_2& insect_start, Point_2& insect_end, bool flag){
            // poly递归终点到起点
            std::vector<Point_2> res_sort;
            VertexCirculator end_iter;
            VertexCirculator iter = polys.vertices_circulator();
            do
            {
                if (std::fabs(CGAL::to_double(iter->x()) - CGAL::to_double(seg_end.x())) < 1e-6 &&
                    std::fabs(CGAL::to_double(iter->y()) - CGAL::to_double(seg_end.y())) < 1e-6)
                {
                    end_iter = iter;
                    if(!flag) res_sort.push_back(insect_start);
                    else res_sort.push_back(insect_end);
                    if (insect_end != seg_end)
                        res_sort.push_back(seg_end);
                    iter++;
                    break;
                }
            } while (++iter != polys.vertices_circulator());
            // 从终点到起点
            do
            {
                if (std::fabs(CGAL::to_double(iter->x()) - CGAL::to_double(seg_start.x())) < 1e-6 &&
                    std::fabs(CGAL::to_double(iter->y()) - CGAL::to_double(seg_start.y())) < 1e-6)
                {
                    res_sort.push_back(seg_start);
                    if (seg_start != insect_start)
                        if(!flag) res_sort.push_back(insect_end);
                        else res_sort.push_back(insect_start);
                    break;
                }
                else
                {
                    res_sort.push_back(*iter);
                }
            } while (++iter != end_iter);

            return res_sort;
        }

        // 交叉点相等时，分别提取轮廓边一定距离作为交叉的起点和终点
        Point_2 Polygon2D::InterceptEdgePoint(Point_2& start, Point_2& end){
            Point_2 special_insect_pt;
            auto dx = CGAL::to_double(end.x()) - CGAL::to_double(start.x());
            auto dy = CGAL::to_double(end.y()) - CGAL::to_double(start.y());
            auto dist = std::sqrt(dx * dx + dy * dy);
            if(dist <= 1.0) special_insect_pt = start;
            else {
                // 截取一定长度后求出交叉起始坐标
                int parts = std::ceil(dist / 1.0);
                auto x = (CGAL::to_double(start.x()) + (parts - 1) * CGAL::to_double(end.x())) / parts;
                auto y = (CGAL::to_double(start.y()) + (parts - 1) * CGAL::to_double(end.y())) / parts;
                special_insect_pt = Point_2(x, y);
            }
            return  special_insect_pt;
        }

        // 合并有交叉的不同多边形
        Polygon_2 Polygon2D::MergeIntersectionPolygons(Polygon_2 &out_polys, Polygon_2 &hole_polys, bool merge)
        {
            Polygon_2 res_out_polys;
            auto out_hole_insects = PolygonsIntersection(out_polys, hole_polys);
            if (out_hole_insects.first.size() <= 0)
                return res_out_polys;

            // 分别统计下不同poly的交叉边
            std::vector<std::pair<EdgeConstIterator, Point_2>> out_edge_insects = out_hole_insects.first;
            std::vector<std::pair<EdgeConstIterator, Point_2>> hole_edge_insects = out_hole_insects.second;

            // out poly按照逆时针顺序排列
            std::vector<std::pair<EdgeConstIterator, Point_2>> order_out_edge_insects = out_edge_insects;

            // hole poly按照顺时针顺序排列
            std::vector<std::pair<EdgeConstIterator, Point_2>> order_hole_edge_insects;
            auto map_order_hole_edge_insects = sortEdgeByIntersectPoints(hole_edge_insects);
            // 统计交叉点个数
            int count_insect = 0;
            for(auto map_order_hole_edge_insect : map_order_hole_edge_insects){
                Point_2 insect(0., 0.);
                for(auto edge_insect : map_order_hole_edge_insect.second){
                    if(insect == edge_insect.second) continue;
                    else{
                        count_insect++;
                        insect = edge_insect.second;
                    }
                }
            }
            // 交叉点超过1退出
            if(map_order_hole_edge_insects.size() > 1 || 
                count_insect == 1 || count_insect > 2){
                return res_out_polys;
            }
            order_hole_edge_insects = map_order_hole_edge_insects[0];

            // 缓存out/hole poly交叉的起点和终点以及交叉点
            Point_2 out_insect_start = order_out_edge_insects.front().first->source();
            Point_2 out_insect_end = order_out_edge_insects.back().first->target();
            if(out_insect_start == out_insect_end) out_insect_end = order_out_edge_insects.back().first->source();
            Point_2 hole_insect_start = order_hole_edge_insects.front().first->source();
            Point_2 hole_insect_end = order_hole_edge_insects.back().first->target();
            if(hole_insect_start == hole_insect_end) hole_insect_end = order_hole_edge_insects.back().first->source();
            Point_2 insect_start = order_hole_edge_insects.back().second;
            Point_2 insect_end = order_hole_edge_insects.front().second;

            // insect_start/end相同时，特殊处理下
            bool s_e_equal = false;
            Point_2 special_out_insect_start, special_out_insect_end;
            Point_2 special_hole_insect_start, special_hole_insect_end;
            if (insect_start == insect_end)
            {
                if(!merge) return res_out_polys;
                special_out_insect_start = InterceptEdgePoint(out_insect_start, insect_start);
                special_out_insect_end = InterceptEdgePoint(out_insect_end, insect_start);
                special_hole_insect_start = InterceptEdgePoint(hole_insect_start, insect_start);
                special_hole_insect_end = InterceptEdgePoint(hole_insect_end, insect_start);
                s_e_equal = true;
            }

            // out poly递归终点到起点
            std::vector<Point_2> res_out;
            VertexCirculator out_end_iter;
            VertexCirculator out_iter = out_polys.vertices_circulator();
            do
            {
                if (std::fabs(CGAL::to_double(out_iter->x()) - CGAL::to_double(out_insect_end.x())) < 1e-6 &&
                    std::fabs(CGAL::to_double(out_iter->y()) - CGAL::to_double(out_insect_end.y())) < 1e-6)
                {
                    out_end_iter = out_iter;
                    if(!s_e_equal) res_out.push_back(insect_end);
                    else res_out.push_back(special_out_insect_end);
                    if (insect_end != out_insect_end)
                        res_out.push_back(out_insect_end);
                    out_iter++;
                    break;
                }
            } while (++out_iter != out_polys.vertices_circulator());
            // 从终点到起点
            do
            {
                if (std::fabs(CGAL::to_double(out_iter->x()) - CGAL::to_double(out_insect_start.x())) < 1e-6 &&
                    std::fabs(CGAL::to_double(out_iter->y()) - CGAL::to_double(out_insect_start.y())) < 1e-6)
                {
                    res_out.push_back(out_insect_start);
                    if (!s_e_equal && out_insect_start != insect_start)
                        res_out.push_back(insect_start);
                    else if(s_e_equal && out_insect_start != special_out_insect_start)
                        res_out.push_back(special_out_insect_start);
                    break;
                }
                else
                {
                    res_out.push_back(*out_iter);
                }
            } while (++out_iter != out_end_iter);

            // hole poly递归终点到起点
            std::vector<Point_2> res_hole;
            VertexCirculator hole_end_iter;
            VertexCirculator hole_iter = hole_polys.vertices_circulator();
            do
            {
                if (std::fabs(CGAL::to_double(hole_iter->x()) - CGAL::to_double(hole_insect_end.x())) < 1e-6 &&
                    std::fabs(CGAL::to_double(hole_iter->y()) - CGAL::to_double(hole_insect_end.y())) < 1e-6)
                {
                    hole_end_iter = hole_iter;
                    if(!s_e_equal) res_hole.emplace_back(insect_start);
                    else res_hole.emplace_back(special_hole_insect_end);
                    if (res_hole[0] != hole_insect_end){
                        res_hole.push_back(hole_insect_end);
                    }
                    hole_iter++;
                    break;
                }
            } while (++hole_iter != hole_polys.vertices_circulator());
            // 从终点到起点
            do
            {
                if (std::fabs(CGAL::to_double(hole_iter->x()) - CGAL::to_double(hole_insect_start.x())) < 1e-6 &&
                    std::fabs(CGAL::to_double(hole_iter->y()) - CGAL::to_double(hole_insect_start.y())) < 1e-6)
                {
                    res_hole.emplace_back(hole_insect_start);
                    if (res_hole[res_hole.size() - 1] != insect_end){
                        if(!s_e_equal) res_hole.push_back(insect_end);
                        else res_hole.push_back(special_hole_insect_start);
                    }
                    break;
                }
                else
                {
                    res_hole.emplace_back(*hole_iter);
                }
            } while (++hole_iter != hole_end_iter);

            // 根据res_out和res_hole的首尾点进行比较，是否需要调整数组的顺序
            if (res_hole.back() == res_out.back()){
                std::reverse(res_hole.begin(), res_hole.end());
            }

            // 生成合并后的多边形
            res_out_polys.insert(res_out_polys.vertices_end(), res_out.begin(), res_out.end());
            res_out_polys.insert(res_out_polys.vertices_end(), res_hole.begin(), res_hole.end());

            // 去重
            for (auto it = res_out_polys.vertices_begin();
                 it != res_out_polys.vertices_end() - 1;)
            {
                if (std::fabs(CGAL::to_double(it->x()) - CGAL::to_double((it + 1)->x())) < 1e-6 &&
                    std::fabs(CGAL::to_double(it->y()) - CGAL::to_double((it + 1)->y())) < 1e-6)
                {
                    it = res_out_polys.erase(it);
                }
                else
                {
                    it++;
                }
            }
            if (std::fabs(CGAL::to_double(res_out_polys.vertices_begin()->x()) - CGAL::to_double((res_out_polys.vertices_end() - 1)->x())) < 1e-6 &&
                std::fabs(CGAL::to_double(res_out_polys.vertices_begin()->y()) - CGAL::to_double((res_out_polys.vertices_end() - 1)->y())) < 1e-6)
            {
                res_out_polys.erase(res_out_polys.vertices_end() - 1);
            }
            return res_out_polys;
        }

        // 合并两个多边形
        Polygon_2 Polygon2D::mergePolygons(const Polygon_2 &poly_1, const Polygon_2 &poly_2)
        {
            // given two polygons that are neighbors, merge them together by creating a new polygon that uses both polygon's
            // points and neighbors
            Polygon_2 new_poly = Polygon_2();

            // first, check to see if either polygon is empty
            if (poly_1.size() == 0)
            {
                new_poly = poly_2;
                return new_poly;
            }
            else if (poly_2.size() == 0)
            {
                new_poly = poly_1;
                return new_poly;
            }

            // we need to find the shared vertices
            VertexConstCirculator poly_1_circulator = poly_1.vertices_circulator();
            VertexConstCirculator poly_1_ending;
            // loop to find a particular shared vertex
            do
            {
                // check if this point in cell 1 is also in cell 2
                if (findPointInPolygon2(*poly_1_circulator, poly_2) != poly_2.vertices_end())
                {
                    // if it is, then it's either the upper or lower intersection. We want whichever comes just after the other, ccw
                    if (findPointInPolygon2(*(poly_1_circulator + 1), poly_2) != poly_2.vertices_end())
                    {
                        // the next vertex is the shared vertex immediately after the other shared vertex!
                        poly_1_circulator++;
                        poly_1_ending = poly_1_circulator;
                        break;
                    }
                }
                poly_1_circulator++;
            } while (poly_1_circulator != poly_1.vertices_circulator());

            // add all of the points from poly_1_circulator to poly_1_circulator - 1 , inclusive, to new_poly
            // this gives us all the points in polygon 1, ending on the leading edge of the boundary between the two polygons
            do
            {
                new_poly.push_back(*poly_1_circulator);
                poly_1_circulator++;
            } while (!math::approximatelyEqual(CGAL::to_double(poly_1_circulator->x()), CGAL::to_double(poly_1_ending->x()), constants::EPSILON) &&
                     !math::approximatelyEqual(CGAL::to_double(poly_1_circulator->y()), CGAL::to_double(poly_1_ending->y()), constants::EPSILON));

            // we've added all of the points in polygon 1
            auto poly_2_circulator = poly_2.vertices_circulator();
            // we need to get poly_2_circulator to point to the same point that we just added to new_poly
            while (!math::approximatelyEqual(CGAL::to_double(poly_2_circulator->x()), CGAL::to_double((poly_1_circulator - 1)->x()), constants::EPSILON) &&
                   !math::approximatelyEqual(CGAL::to_double(poly_2_circulator->y()), CGAL::to_double((poly_1_circulator - 1)->y()), constants::EPSILON))
            {
                poly_2_circulator++;
            }

            // now add all the points from poly_2_circulator to poly_1_circulator + 1, exclusive, to new_poly
            poly_2_circulator++;
            while (!math::approximatelyEqual(CGAL::to_double(poly_2_circulator->x()), CGAL::to_double(poly_1_circulator->x()), constants::EPSILON) &&
                   !math::approximatelyEqual(CGAL::to_double(poly_2_circulator->y()), CGAL::to_double(poly_1_circulator->y()), constants::EPSILON))
            {
                new_poly.push_back(*poly_2_circulator);
                poly_2_circulator++;
            }

            // this is the complete new poly!
            return new_poly;
        }

        // 点插入到多边形
        void Polygon2D::insertPointAlongEdge(const Point_2 &point, Polygon_2 &poly)
        {
            // check to see if point is a vertex
            if (findPointInPolygon2(point, poly) != poly.vertices_end())
            {
                // it was a vertex, we don't have to do anything
                return;
            }

            // the point intersects an edge, we need to find that edge
            auto edge_iterator = poly.edges_begin();
            for (; edge_iterator != poly.edges_end(); edge_iterator++)
            {
                Segment_2 edge = *edge_iterator;
                // check the point's distance to the edge - it might be off by a little bit because of floating point error
                if (math::definitelyLessThan(CGAL::to_double(CGAL::squared_distance(point, edge)), constants::EPSILON, constants::EPSILON))
                {
                    // we found the edge!
                    break;
                }
            }

            // check to see if we found the edge
            if (edge_iterator == poly.edges_end())
            {
                std::cout << "couldn't find the edge that point " << point.x() << " , " << point.y() << " lies on!" << std::endl;
                return;
            }

            // now, we need to modify the polygon such that we have two edges from source -> point and point -> target
            // we can do this by simply inserting point after the edge's source
            for (auto point_iterator = poly.vertices_begin(); point_iterator != poly.vertices_end(); point_iterator++)
            {
                if (math::approximatelyEqual(CGAL::to_double(point_iterator->x()), CGAL::to_double(edge_iterator->source().x()), constants::EPSILON) &&
                    math::approximatelyEqual(CGAL::to_double(point_iterator->y()), CGAL::to_double(edge_iterator->source().y()), constants::EPSILON))
                {
                    // we found the source point in the points list!
                    // we'll insert the point -- this will invalidate the iterator but that's ok because we're done with it
                    point_iterator++;
                    poly.insert(point_iterator, point);
                    return;
                }
            }
        }

        // 获取>180度的顶点作为关键点
        std::vector<Point_2> Polygon2D::getCriticalInflectionPoints(Polygon_2 &polygon)
        {
            // ensure that the polygon is counter_clockwise oriented. Later functions require this.
            if (polygon.is_clockwise_oriented())
            {
                polygon.reverse_orientation();
            }

            std::vector<Point_2> critical_inflection_points;
            auto vertex = polygon.vertices_circulator();

            // loop through all vertices of the polygon
            do
            {
                // for each vertex, check its interior angle. If greater than 180 degrees, add it to the list
                double interior_angle = getInteriorAngleOfVertex(vertex);
                if (interior_angle > 180.0)
                {
                    critical_inflection_points.push_back(*vertex);
                }

                vertex++;
            } while (vertex != polygon.vertices_circulator());

            // return the list
            return critical_inflection_points;
        }

        // 获取顶点的角度
        double Polygon2D::getInteriorAngleOfVertex(const VertexConstCirculator &vertex)
        {
            // check if vertex is a critical point
            // because the polygon is counter_clockwise oriented, interior angle will be counter clockwise from prev to next
            auto prev_vertex = vertex - 1;
            auto next_vertex = vertex + 1;

            // make vectors from vertex to previous and next points
            Vector_2 v1 = *prev_vertex - *vertex;
            Vector_2 v2 = *next_vertex - *vertex;

            // get the atan2 angle of each vector to x-axis vector, radians in range -PI to PI
            double alpha = std::atan2(CGAL::to_double(v1.y()), CGAL::to_double(v1.x()));
            double beta = std::atan2(CGAL::to_double(v2.y()), CGAL::to_double(v2.x()));

            // transform alpha and beta to the range 0 to 2PI
            if (alpha < 0)
            {
                alpha += (CGAL_PI * 2);
            }
            if (beta < 0)
            {
                beta += (CGAL_PI * 2);
            }

            // use alpha and beta to calculate the clockwise-angle between alpha and beta
            double angle;
            if (alpha > beta)
            {
                angle = alpha - beta;
            }
            else if (alpha < beta)
            {
                angle = (CGAL_PI * 2) - beta + alpha;
            }
            else
            {
                angle = 0.0;
            }

            // scale angle to 0 to 360 in degrees
            angle *= 180 / CGAL_PI;

            // return angle
            return angle;
        }
        // 获取多边形所有的边
        std::vector<Direction_2> Polygon2D::getPolygonEdgeDirections(const Polygon_2 &poly)
        {
            // Get all directions.
            std::vector<Direction_2> dirs;
            for (EdgeConstIterator it = poly.edges_begin(); it != poly.edges_end(); ++it)
            {
                dirs.push_back(it->direction());
            }
            return dirs;
        }

    } // namespace ccpp
} // global_plan_path